clear all;
%run('ukfQ.m')

run('C:\Users\GrubyGrub\Dropbox\robotics\sr-cdkf\ukfQ.m')

%%
magC = [0.00200514834530657      0.000145660478826823    -2.28229956453876e-05
        3.49857099645271e-07     0.00219537371085451      3.24479067318367e-05
       -5.48178004495311e-08     8.13470415420607e-08     0.00184810513348282];

magM = [-42.163146046205
        -191.918971854855
        -178.53760333671];

gyroI = [gyroI gyroI];
accI = [accI accI];
magI = [magI magI];

gyroI = [gyroI gyroI];
accI = [accI accI];
magI = [magI magI];

[~, N] = size(magI);

%Model parameters
modelObject.statedim = 7; %state dimension
modelObject.U1dim = 3;  %exogenous input dimension
modelObject.Vdim = 6; %State noise dimension
modelObject.scaleFactor = sqrt(3);

Xh = zeros(modelObject.statedim,1);          % state estimation buffer
Xh(:,1) = zeros(modelObject.statedim,1);
Xh(1,1) = 1;%
Xh = [ 0.1739
    -0.0216
    0.0295
    0.9834
    0
    0
    0];

%process noise
pNoise.dim = 6;
pNoise.mu  = ones(pNoise.dim,1) * 0;
pNoise.cov   = sqrt(.0000505)*eye(pNoise.dim);

%Acclermeter noise
oNoiseAcc.dim = 3;
oNoiseAcc.mu  = ones(oNoiseAcc.dim,1) * 0;
oNoiseAcc.cov   = sqrt(.000165)*eye(oNoiseAcc.dim);
oNoiseAcc.hfun = @acc_hfun;

%Magneto noise
oNoiseMag.dim = 3;
oNoiseMag.mu  = ones(oNoiseMag.dim,1) * 0;
oNoiseMag.cov   = sqrt(.0085)*eye(oNoiseMag.dim);
oNoiseMag.hfun = @mag_hfun;

% initial estimate of state E[X(0)]
Px = pNoise.cov(1,1)*eye(modelObject.statedim);          % initial state covariance
InfDS.spkfParams  = sqrt(3);    % scale factor (CDKF parameter h)

Sx = Px;

for z = 1:N
    
    y = [accI(:,z); magI(:,z)];
    U1 = gyroI(:,z);
    
    [Xh, Sy] = ukf_measurement(Xh, Sx, pNoise, U1, modelObject);
     if mod(z,1) == 0
        [Xh, Sx] = ukf_observation(Xh, Sy, oNoiseAcc,accI(:,z), [], modelObject);
     end
    if mod(z,7) == 0
   %     [Xh, Sx] = ukf_observation(Xh, Sy, oNoiseMag,magI(:,z), [], modelObject);
    end
    
    state(:,z) = Xh;
    
end
Xh = state;


%%

X  = zeros(modelObject.statedim,N);                           % state data buffer
y  = zeros(modelObject.statedim,N);                             % observation data buffer
X(:,1) = zeros(modelObject.statedim,1);
X(1,1) = 1;%plot
for j=2:N,
    X(:,j) = gyroRaw( [], X(:,j-1), gyroI(:,j-1), []);
end

%Filtered results
for k=1:N
    
    q0 = Xh(1,k);
    q1 = Xh(2,k);
    q2 = Xh(3,k);
    q3 = Xh(4,k);
    
    %Z-measurement of the rotation matrix
    qfx(k) =  2*(q1*q3 - q0*q2);
    qfy(k) =  2*(q2*q3 + q0*q1);
    qfz(k)  = 1 - 2*(q1*q1 + q2*q2);
    
    
    
    
    %magno values
    qmx(k) = 1-2*(q2*q2 + q3*q3);
    qmy(k) =2*(q1*q2 - q0*q3);
    qmz(k) = 2*(q0*q2 + q1*q3);
    
    
    rollF(k) = atan2(2*(q0*q1+q2*q3), 1-2*(q1^2+q2^2)) * 180 / pi;
    pitchF(k) = -asin(2*(q0*q2-q3*q1))* 180 / pi;
    yawF(k) = atan2(2*(q0*q3+q1*q2), 1-2*(q2^2+q3^2))* 180 / pi;
end


%Quaternion gyroscope and acclemeter results
for k=1:N
    q0 = X(1,k);
    q1 = X(2,k);
    q2 = X(3,k);
    q3 = X(4,k);
    rollR(k) = atan2(2*(q0*q1+q2*q3), 1-2*(q1^2+q2^2)) * 180 / pi;
    pitchR(k) = -asin(2*(q0*q2-q3*q1))* 180 / pi;
    yawR(k) = atan2(2*(q0*q3+q1*q2), 1-2*(q2^2+q3^2))* 180 / pi;
    
    %Acclmenter from gyro
    qx(k) =  2*(q1*q3 - q0*q2);
    qy(k) =  2*(q2*q3 + q0*q1);
    qz(k)  = 1 - 2*(q1*q1 + q2*q2);
    
    rollQ(k) = atan2(qy(k),(sqrt(qz(k)^2+qz(k)^2))) * 180 / pi;
    pitchQ(k) = atan2(qx(k),sqrt(qy(k)^2+qz(k)^2)) *180/pi;
end

%Raw acclemeter values
for k=1:N
    gx = accI(1,k);
    gy = accI(2,k);
    gz = accI(3,k);
    
    rollA(k) = atan2(gy,(sqrt(gz^2+gz^2))) * 180 / pi;
    pitchA(k) = atan2(gx,sqrt(gy^2+gz^2)) *180/pi;
    
    
end


figure(1); clf;
subplot(3,1,1), plot(pitchF(1,:),'r','LineWidth',2); hold on
subplot(3,1,1),plot(pitchR(1,:),'g');
subplot(3,1,1),plot(pitchA(1,:),'b+'); hold off;

subplot(3,1,2), plot(rollF(1,:),'r','LineWidth',2); hold on
subplot(3,1,2), plot(rollR(1,:),'g');
subplot(3,1,2), plot(rollA(1,:),'b+'); hold off;

%
subplot(3,1,3), plot(yawF(1,:),'r','LineWidth',2); hold on
subplot(3,1,3), plot(yawR(1,:),'g');
subplot(3,1,3), plot(-atan2(magI(2,1:N),magI(1,1:N))*180/pi,'b'); hold off;


drawnow
